#ifndef FOLLOWER_H_
#define FOLLOWER_H_

#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include "Driver.h"
#include "utilities/Event.h"
#include "utilities/ThreadSafeQueue.h"
#include "utilities/Constants.h"
#include <string>
#include <math.h>       /* atan */
using namespace std;

#define MSG_QUEUE_SIZE 5

/**
 * Implements a wall following algorithm:
 * keeps indicated distance from wall, while driving at given speed
 */
class WallFollower {

private:

	// ROS management objects
	ros::NodeHandle m_node_handler;
	ros::Rate* m_loop_rate;
	ros::Rate* m_fast_loop_rate;
	ros::Publisher m_sensors_publisher;
	std_msgs::String m_message_to_publish;
	ros::Subscriber sub_sensors_frl;
	ros::Subscriber sub_sensors_bl;
	ros::Subscriber sub_sensors_back_us;
	ros::Subscriber sub_sensors_rl;
	ros::Subscriber sub_sensors_right_us;
	ros::Subscriber sub_sensors_ll;
	ros::Subscriber sub_sensors_left_us;
	ros::Subscriber sub_sensors_fl;

	// utility objects
	ThreadSafeQueue<Event> event_queue;
	Driver* m_driver;
	double m_speed; // robot's speed while following wall
	double m_goalWallDist; // distance to keep from wall
	double m_sideDist; // distance measured from side sensor
	std::mutex m_mutex; // used for synchronized access to data

	/**
	 * Finds initial driving direction
	 */
	void startDriving();

	/**
	 * Maintains parallel driving at indicated distance from wall
	 */
	void maintainDirection();

	/**
	 * Fixes robot's distance from wall:
	 * if too close, turns towards the wall ; if too far, turns away from the wall
	 */
	void fixDistance(const Event& currEvent);

	/**
	 * Calculates how much should turn (in radians) so that the robot
	 * will be parallel to the wall
	 */
	double align(const Event& event);

	/**
	 * Calculates how much should turn (in radians) in order to correct
	 * distance from wall (either too close or too far)
	 */
	double turnAngle(double horizontalDist);

	/**
	 * Returns distance sampled from side sensor.
	 * Method is synchronized because sensor's handler keeps updating variable
	 */
	double getSideDistance();

	/**
	 * Sets current distance sampled from side sensor.
	 * Method is synchronized because algorithm keeps reading variable
	 */
	void setSideDistance(double newDist);


	bool isClose(const Event& event);
	bool isFar(const Event& event);
	bool isSensorPending(const Event& event);
	bool isSensorFront(const Event& event);
	double boostAngle(double fixAngle);
	bool shouldDriveAway(const Event& front_right_lsr_dummy);
	bool shouldDriveCloser(const Event& front_right_lsr_dummy);

public:

	/**
	 * Subscribes to relevant sensors for the wall following
	 */
	void subscribeToSensor(sensors_names::ordinal sensor,
			bool shouldSubscribe, double min_distance, double max_distance);

	// handler methods- create events according to sampled data
	void frontLaserHandler(const std_msgs::String::ConstPtr& msg);
	void frontRightLaserHandler(const std_msgs::String::ConstPtr& msg);
	void backLaserHandler(const std_msgs::String::ConstPtr& msg);
	void backUSHandler(const std_msgs::String::ConstPtr& msg);
	void rightUSHandler(const std_msgs::String::ConstPtr& msg);
	void rightLaserHandler(const std_msgs::String::ConstPtr& msg);
	void leftUSHandler(const std_msgs::String::ConstPtr& msg);
	void leftLaserHandler(const std_msgs::String::ConstPtr& msg);

	/**
	 * Constructor- initializes a new object to manage wall following
	 */
	WallFollower(Driver* driver, double speed, double wallDistance);

	/**
	 * Reads data from event queue and responds accordingly
	 */
	void follow();
};

#endif
